{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "!curl -L https://raw.githubusercontent.com/facebookresearch/habitat-sim/master/examples/colab_utils/colab_install.sh | NIGHTLY=true bash -s"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "%cd /content/habitat-sim\n",
    "## [setup]\n",
    "import math\n",
    "import os\n",
    "import random\n",
    "import sys\n",
    "\n",
    "import cv2\n",
    "import git\n",
    "import magnum as mn\n",
    "import numpy as np\n",
    "\n",
    "import habitat_sim\n",
    "from habitat_sim.utils import viz_utils as vut\n",
    "\n",
    "if \"google.colab\" in sys.modules:\n",
    "    os.environ[\"IMAGEIO_FFMPEG_EXE\"] = \"/usr/bin/ffmpeg\"\n",
    "\n",
    "repo = git.Repo(\".\", search_parent_directories=True)\n",
    "dir_path = repo.working_tree_dir\n",
    "%cd $dir_path\n",
    "data_path = os.path.join(dir_path, \"data\")\n",
    "output_path = os.path.join(dir_path, \"examples/tutorials/rigid_object_tutorial_output/\")\n",
    "\n",
    "\n",
    "def make_video_cv2(observations, prefix=\"\", open_vid=True, multi_obs=False):\n",
    "    videodims = (720, 544)\n",
    "    video_file = output_path + prefix + \".mp4\"\n",
    "    print(\"Encoding the video: %s \" % video_file)\n",
    "    writer = vut.get_fast_video_writer(video_file, fps=60)\n",
    "    thumb_size = (int(videodims[0] / 5), int(videodims[1] / 5))\n",
    "    outline_frame = np.ones((thumb_size[1] + 2, thumb_size[0] + 2, 3), np.uint8) * 150\n",
    "    for ob in observations:\n",
    "\n",
    "        # If in RGB/RGBA format, remove the alpha channel\n",
    "        rgb_im_1st_person = cv2.cvtColor(\n",
    "            ob[\"rgba_camera_1stperson\"], cv2.COLOR_RGBA2RGB\n",
    "        )\n",
    "\n",
    "        if multi_obs:\n",
    "            # embed the 1st person RBG frame into the 3rd person frame\n",
    "            rgb_im_3rd_person = cv2.cvtColor(\n",
    "                ob[\"rgba_camera_3rdperson\"], cv2.COLOR_RGBA2RGB\n",
    "            )\n",
    "            resized_1st_person_rgb = cv2.resize(\n",
    "                rgb_im_1st_person, thumb_size, interpolation=cv2.INTER_AREA\n",
    "            )\n",
    "            x_offset = 50\n",
    "            y_offset_rgb = 50\n",
    "            rgb_im_3rd_person[\n",
    "                y_offset_rgb - 1 : y_offset_rgb + outline_frame.shape[0] - 1,\n",
    "                x_offset - 1 : x_offset + outline_frame.shape[1] - 1,\n",
    "            ] = outline_frame\n",
    "            rgb_im_3rd_person[\n",
    "                y_offset_rgb : y_offset_rgb + resized_1st_person_rgb.shape[0],\n",
    "                x_offset : x_offset + resized_1st_person_rgb.shape[1],\n",
    "            ] = resized_1st_person_rgb\n",
    "\n",
    "            # embed the 1st person DEPTH frame into the 3rd person frame\n",
    "            # manually normalize depth into [0, 1] so that images are always consistent\n",
    "            d_im = np.clip(ob[\"depth_camera_1stperson\"], 0, 10)\n",
    "            d_im /= 10.0\n",
    "            bgr_d_im = cv2.cvtColor((d_im * 255).astype(np.uint8), cv2.COLOR_GRAY2RGB)\n",
    "            resized_1st_person_depth = cv2.resize(\n",
    "                bgr_d_im, thumb_size, interpolation=cv2.INTER_AREA\n",
    "            )\n",
    "            y_offset_d = y_offset_rgb + 10 + thumb_size[1]\n",
    "            rgb_im_3rd_person[\n",
    "                y_offset_d - 1 : y_offset_d + outline_frame.shape[0] - 1,\n",
    "                x_offset - 1 : x_offset + outline_frame.shape[1] - 1,\n",
    "            ] = outline_frame\n",
    "            rgb_im_3rd_person[\n",
    "                y_offset_d : y_offset_d + resized_1st_person_depth.shape[0],\n",
    "                x_offset : x_offset + resized_1st_person_depth.shape[1],\n",
    "            ] = resized_1st_person_depth\n",
    "            if rgb_im_3rd_person.shape[:2] != videodims:\n",
    "                rgb_im_3rd_person = cv2.resize(\n",
    "                    rgb_im_3rd_person, videodims, interpolation=cv2.INTER_AREA\n",
    "                )\n",
    "            # write the video frame\n",
    "            writer.append_data(rgb_im_3rd_person)\n",
    "        else:\n",
    "            if rgb_im_1st_person.shape[:2] != videodims:\n",
    "                rgb_im_1st_person = cv2.resize(\n",
    "                    rgb_im_1st_person, videodims, interpolation=cv2.INTER_AREA\n",
    "                )\n",
    "            # write the 1st person observation to video\n",
    "            writer.append_data(rgb_im_1st_person)\n",
    "    writer.close()\n",
    "\n",
    "    if open_vid:\n",
    "        print(\"Displaying video\")\n",
    "        vut.display_video(video_file)\n",
    "\n",
    "\n",
    "def remove_all_objects(sim):\n",
    "    for id in sim.get_existing_object_ids():\n",
    "        sim.remove_object(id)\n",
    "\n",
    "\n",
    "def place_agent(sim):\n",
    "    # place our agent in the scene\n",
    "    agent_state = habitat_sim.AgentState()\n",
    "    agent_state.position = [-0.15, -0.7, 1.0]\n",
    "    # agent_state.position = [-0.15, -1.6, 1.0]\n",
    "    agent_state.rotation = np.quaternion(-0.83147, 0, 0.55557, 0)\n",
    "    agent = sim.initialize_agent(0, agent_state)\n",
    "    return agent.scene_node.transformation_matrix()\n",
    "\n",
    "\n",
    "def make_configuration():\n",
    "    # simulator configuration\n",
    "    backend_cfg = habitat_sim.SimulatorConfiguration()\n",
    "    backend_cfg.scene.id = os.path.join(\n",
    "        data_path, \"scene_datasets/habitat-test-scenes/apartment_1.glb\"\n",
    "    )\n",
    "    assert os.path.exists(backend_cfg.scene.id)\n",
    "    backend_cfg.enable_physics = True\n",
    "\n",
    "    # sensor configurations\n",
    "    # Note: all sensors must have the same resolution\n",
    "    # setup 2 rgb sensors for 1st and 3rd person views\n",
    "    camera_resolution = [544, 720]\n",
    "    sensors = {\n",
    "        \"rgba_camera_1stperson\": {\n",
    "            \"sensor_type\": habitat_sim.SensorType.COLOR,\n",
    "            \"resolution\": camera_resolution,\n",
    "            \"position\": [0.0, 0.6, 0.0],\n",
    "            \"orientation\": [0.0, 0.0, 0.0],\n",
    "        },\n",
    "        \"depth_camera_1stperson\": {\n",
    "            \"sensor_type\": habitat_sim.SensorType.DEPTH,\n",
    "            \"resolution\": camera_resolution,\n",
    "            \"position\": [0.0, 0.6, 0.0],\n",
    "            \"orientation\": [0.0, 0.0, 0.0],\n",
    "        },\n",
    "        \"rgba_camera_3rdperson\": {\n",
    "            \"sensor_type\": habitat_sim.SensorType.COLOR,\n",
    "            \"resolution\": camera_resolution,\n",
    "            \"position\": [0.0, 1.0, 0.3],\n",
    "            \"orientation\": [-45, 0.0, 0.0],\n",
    "        },\n",
    "    }\n",
    "\n",
    "    sensor_specs = []\n",
    "    for sensor_uuid, sensor_params in sensors.items():\n",
    "        sensor_spec = habitat_sim.SensorSpec()\n",
    "        sensor_spec.uuid = sensor_uuid\n",
    "        sensor_spec.sensor_type = sensor_params[\"sensor_type\"]\n",
    "        sensor_spec.resolution = sensor_params[\"resolution\"]\n",
    "        sensor_spec.position = sensor_params[\"position\"]\n",
    "        sensor_spec.orientation = sensor_params[\"orientation\"]\n",
    "        sensor_specs.append(sensor_spec)\n",
    "\n",
    "    # agent configuration\n",
    "    agent_cfg = habitat_sim.agent.AgentConfiguration()\n",
    "    agent_cfg.sensor_specifications = sensor_specs\n",
    "\n",
    "    return habitat_sim.Configuration(backend_cfg, [agent_cfg])\n",
    "\n",
    "\n",
    "def simulate(sim, dt=1.0, get_frames=True):\n",
    "    # simulate dt seconds at 60Hz to the nearest fixed timestep\n",
    "    print(\"Simulating \" + str(dt) + \" world seconds.\")\n",
    "    observations = []\n",
    "    start_time = sim.get_world_time()\n",
    "    while sim.get_world_time() < start_time + dt:\n",
    "        sim.step_physics(1.0 / 60.0)\n",
    "        if get_frames:\n",
    "            observations.append(sim.get_sensor_observations())\n",
    "\n",
    "    return observations\n",
    "\n",
    "\n",
    "# [/setup]\n",
    "if __name__ == \"__main__\":\n",
    "    import argparse\n",
    "\n",
    "    parser = argparse.ArgumentParser()\n",
    "    parser.add_argument(\"--no-show-video\", dest=\"show_video\", action=\"store_false\")\n",
    "    parser.add_argument(\"--no-make-video\", dest=\"make_video\", action=\"store_false\")\n",
    "    parser.set_defaults(show_video=True, make_video=True)\n",
    "    args, _ = parser.parse_known_args()\n",
    "    show_video = args.show_video\n",
    "    make_video = args.make_video\n",
    "    if make_video:\n",
    "        if not os.path.exists(output_path):\n",
    "            os.mkdir(output_path)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "    # [initialize]\n",
    "    # create the simulators AND resets the simulator\n",
    "\n",
    "    cfg = make_configuration()\n",
    "    try:  # Got to make initialization idiot proof\n",
    "        sim.close()\n",
    "    except NameError:\n",
    "        pass\n",
    "    sim = habitat_sim.Simulator(cfg)\n",
    "    agent_transform = place_agent(sim)\n",
    "\n",
    "    # get the primitive assets attributes manager\n",
    "    prim_templates_mgr = sim.get_asset_template_manager()\n",
    "\n",
    "    # get the physics object attributes manager\n",
    "    obj_templates_mgr = sim.get_object_template_manager()\n",
    "    # [/initialize]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "lines_to_next_cell": 0
   },
   "outputs": [],
   "source": [
    "    # [basics]\n",
    "\n",
    "    # load some object templates from configuration files\n",
    "    sphere_template_id = obj_templates_mgr.load_object_configs(\n",
    "        str(os.path.join(data_path, \"test_assets/objects/sphere\"))\n",
    "    )[0]\n",
    "\n",
    "    # add an object to the scene\n",
    "    id_1 = sim.add_object(sphere_template_id)\n",
    "    sim.set_translation(np.array([2.50, 0, 0.2]), id_1)\n",
    "\n",
    "    # simulate\n",
    "    observations = simulate(sim, dt=1.5, get_frames=make_video)\n",
    "\n",
    "    if make_video:\n",
    "        make_video_cv2(observations, prefix=\"sim_basics\", open_vid=show_video)\n",
    "\n",
    "    # [/basics]\n",
    "\n",
    "    remove_all_objects(sim)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "lines_to_next_cell": 0
   },
   "outputs": [],
   "source": [
    "    # [dynamic_control]\n",
    "\n",
    "    observations = []\n",
    "    # search for an object template by key sub-string\n",
    "    cheezit_template_handle = obj_templates_mgr.get_template_handles(\n",
    "        \"data/objects/cheezit\"\n",
    "    )[0]\n",
    "    box_positions = [\n",
    "        np.array([2.39, -0.37, 0]),\n",
    "        np.array([2.39, -0.64, 0]),\n",
    "        np.array([2.39, -0.91, 0]),\n",
    "        np.array([2.39, -0.64, -0.22]),\n",
    "        np.array([2.39, -0.64, 0.22]),\n",
    "    ]\n",
    "    box_orientation = mn.Quaternion.rotation(\n",
    "        mn.Rad(math.pi / 2.0), np.array([-1.0, 0, 0])\n",
    "    )\n",
    "    # instance and place the boxes\n",
    "    box_ids = []\n",
    "    for b in range(5):\n",
    "        box_ids.append(sim.add_object_by_handle(cheezit_template_handle))\n",
    "        sim.set_translation(box_positions[b], box_ids[b])\n",
    "        sim.set_rotation(box_orientation, box_ids[b])\n",
    "\n",
    "    # get the object's initialization attributes (all boxes initialized with same mass)\n",
    "    object_init_template = sim.get_object_initialization_template(box_ids[0])\n",
    "    # anti-gravity force f=m(-g)\n",
    "    anti_grav_force = -1.0 * sim.get_gravity() * object_init_template.mass\n",
    "\n",
    "    # throw a sphere at the boxes from the agent position\n",
    "    sphere_template = obj_templates_mgr.get_template_by_ID(sphere_template_id)\n",
    "    sphere_template.scale = np.array([0.5, 0.5, 0.5])\n",
    "    obj_templates_mgr.register_template(sphere_template)\n",
    "\n",
    "    sphere_id = sim.add_object(sphere_template_id)\n",
    "    sim.set_translation(\n",
    "        sim.agents[0].get_state().position + np.array([0, 1.0, 0]), sphere_id\n",
    "    )\n",
    "    # get the vector from the sphere to a box\n",
    "    target_direction = sim.get_translation(box_ids[0]) - sim.get_translation(sphere_id)\n",
    "    # apply an initial velocity for one step\n",
    "    sim.set_linear_velocity(target_direction * 5, sphere_id)\n",
    "    sim.set_angular_velocity(np.array([0, -1.0, 0]), sphere_id)\n",
    "\n",
    "    start_time = sim.get_world_time()\n",
    "    dt = 3.0\n",
    "    while sim.get_world_time() < start_time + dt:\n",
    "        # set forces/torques before stepping the world\n",
    "        for box_id in box_ids:\n",
    "            sim.apply_force(anti_grav_force, np.array([0, 0.0, 0]), box_id)\n",
    "            sim.apply_torque(np.array([0, 0.01, 0]), box_id)\n",
    "        sim.step_physics(1.0 / 60.0)\n",
    "        observations.append(sim.get_sensor_observations())\n",
    "\n",
    "    if make_video:\n",
    "        make_video_cv2(observations, prefix=\"dynamic_control\", open_vid=show_video)\n",
    "\n",
    "    # [/dynamic_control]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "lines_to_next_cell": 0
   },
   "outputs": [],
   "source": [
    "    # [kinematic_interactions]\n",
    "\n",
    "    chefcan_template_handle = obj_templates_mgr.get_template_handles(\n",
    "        \"data/objects/chefcan\"\n",
    "    )[0]\n",
    "    id_1 = sim.add_object_by_handle(chefcan_template_handle)\n",
    "    sim.set_translation(np.array([2.4, -0.64, 0]), id_1)\n",
    "    # set one object to kinematic\n",
    "    sim.set_object_motion_type(habitat_sim.physics.MotionType.KINEMATIC, id_1)\n",
    "\n",
    "    # drop some dynamic objects\n",
    "    id_2 = sim.add_object_by_handle(chefcan_template_handle)\n",
    "    sim.set_translation(np.array([2.4, -0.64, 0.28]), id_2)\n",
    "    id_3 = sim.add_object_by_handle(chefcan_template_handle)\n",
    "    sim.set_translation(np.array([2.4, -0.64, -0.28]), id_3)\n",
    "    id_4 = sim.add_object_by_handle(chefcan_template_handle)\n",
    "    sim.set_translation(np.array([2.4, -0.3, 0]), id_4)\n",
    "\n",
    "    # simulate\n",
    "    observations = simulate(sim, dt=1.5, get_frames=True)\n",
    "\n",
    "    if make_video:\n",
    "        make_video_cv2(\n",
    "            observations, prefix=\"kinematic_interactions\", open_vid=show_video\n",
    "        )\n",
    "\n",
    "    # [/kinematic_interactions]\n",
    "\n",
    "    remove_all_objects(sim)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "lines_to_next_cell": 0
   },
   "outputs": [],
   "source": [
    "    # [kinematic_update]\n",
    "    observations = []\n",
    "\n",
    "    clamp_template_handle = obj_templates_mgr.get_template_handles(\n",
    "        \"data/objects/largeclamp\"\n",
    "    )[0]\n",
    "    id_1 = sim.add_object_by_handle(clamp_template_handle)\n",
    "    sim.set_object_motion_type(habitat_sim.physics.MotionType.KINEMATIC, id_1)\n",
    "    sim.set_translation(np.array([0.8, 0, 0.5]), id_1)\n",
    "\n",
    "    start_time = sim.get_world_time()\n",
    "    dt = 1.0\n",
    "    while sim.get_world_time() < start_time + dt:\n",
    "        # manually control the object's kinematic state\n",
    "        sim.set_translation(sim.get_translation(id_1) + np.array([0, 0, 0.01]), id_1)\n",
    "        sim.set_rotation(\n",
    "            mn.Quaternion.rotation(mn.Rad(0.05), np.array([-1.0, 0, 0]))\n",
    "            * sim.get_rotation(id_1),\n",
    "            id_1,\n",
    "        )\n",
    "        sim.step_physics(1.0 / 60.0)\n",
    "        observations.append(sim.get_sensor_observations())\n",
    "\n",
    "    if make_video:\n",
    "        make_video_cv2(observations, prefix=\"kinematic_update\", open_vid=show_video)\n",
    "\n",
    "    # [/kinematic_update]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "lines_to_next_cell": 0
   },
   "outputs": [],
   "source": [
    "    # [velocity_control]\n",
    "\n",
    "    # get object VelocityControl structure and setup control\n",
    "    vel_control = sim.get_object_velocity_control(id_1)\n",
    "    vel_control.linear_velocity = np.array([0, 0, -1.0])\n",
    "    vel_control.angular_velocity = np.array([4.0, 0, 0])\n",
    "    vel_control.controlling_lin_vel = True\n",
    "    vel_control.controlling_ang_vel = True\n",
    "\n",
    "    observations = simulate(sim, dt=1.0, get_frames=True)\n",
    "\n",
    "    # reverse linear direction\n",
    "    vel_control.linear_velocity = np.array([0, 0, 1.0])\n",
    "\n",
    "    observations += simulate(sim, dt=1.0, get_frames=True)\n",
    "\n",
    "    if make_video:\n",
    "        make_video_cv2(observations, prefix=\"velocity_control\", open_vid=show_video)\n",
    "\n",
    "    # [/velocity_control]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "lines_to_next_cell": 0
   },
   "outputs": [],
   "source": [
    "    # [local_velocity_control]\n",
    "\n",
    "    vel_control.linear_velocity = np.array([0, 0, 2.3])\n",
    "    vel_control.angular_velocity = np.array([-4.3, 0.0, 0])\n",
    "    vel_control.lin_vel_is_local = True\n",
    "    vel_control.ang_vel_is_local = True\n",
    "\n",
    "    observations = simulate(sim, dt=1.5, get_frames=True)\n",
    "\n",
    "    # video rendering\n",
    "    if make_video:\n",
    "        make_video_cv2(\n",
    "            observations, prefix=\"local_velocity_control\", open_vid=show_video\n",
    "        )\n",
    "\n",
    "    # [/local_velocity_control]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "    # [embodied_agent]\n",
    "\n",
    "    # load the lobot_merged asset\n",
    "    locobot_template_id = obj_templates_mgr.load_object_configs(\n",
    "        str(os.path.join(data_path, \"objects/locobot_merged\"))\n",
    "    )[0]\n",
    "\n",
    "    # add robot object to the scene with the agent/camera SceneNode attached\n",
    "    id_1 = sim.add_object(locobot_template_id, sim.agents[0].scene_node)\n",
    "    sim.set_translation(np.array([1.75, -1.02, 0.4]), id_1)\n",
    "\n",
    "    vel_control = sim.get_object_velocity_control(id_1)\n",
    "    vel_control.linear_velocity = np.array([0, 0, -1.0])\n",
    "    vel_control.angular_velocity = np.array([0.0, 2.0, 0])\n",
    "\n",
    "    # simulate robot dropping into place\n",
    "    observations = simulate(sim, dt=1.5, get_frames=make_video)\n",
    "\n",
    "    vel_control.controlling_lin_vel = True\n",
    "    vel_control.controlling_ang_vel = True\n",
    "    vel_control.lin_vel_is_local = True\n",
    "    vel_control.ang_vel_is_local = True\n",
    "\n",
    "    # simulate forward and turn\n",
    "    observations += simulate(sim, dt=1.0, get_frames=make_video)\n",
    "\n",
    "    vel_control.controlling_lin_vel = False\n",
    "    vel_control.angular_velocity = np.array([0.0, 1.0, 0])\n",
    "\n",
    "    # simulate turn only\n",
    "    observations += simulate(sim, dt=1.5, get_frames=make_video)\n",
    "\n",
    "    vel_control.angular_velocity = np.array([0.0, 0.0, 0])\n",
    "    vel_control.controlling_lin_vel = True\n",
    "    vel_control.controlling_ang_vel = True\n",
    "\n",
    "    # simulate forward only with damped angular velocity (reset angular velocity to 0 after each step)\n",
    "    observations += simulate(sim, dt=1.0, get_frames=make_video)\n",
    "\n",
    "    vel_control.angular_velocity = np.array([0.0, -1.25, 0])\n",
    "\n",
    "    # simulate forward and turn\n",
    "    observations += simulate(sim, dt=2.0, get_frames=make_video)\n",
    "\n",
    "    vel_control.controlling_ang_vel = False\n",
    "    vel_control.controlling_lin_vel = False\n",
    "\n",
    "    # simulate settling\n",
    "    observations += simulate(sim, dt=3.0, get_frames=make_video)\n",
    "\n",
    "    # remove the agent's body while preserving the SceneNode\n",
    "    sim.remove_object(id_1, False)\n",
    "\n",
    "    # video rendering with embedded 1st person view\n",
    "    if make_video:\n",
    "        make_video_cv2(\n",
    "            observations, prefix=\"robot_control\", open_vid=show_video, multi_obs=True\n",
    "        )\n",
    "\n",
    "    # [/embodied_agent]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "    # [embodied_agent_navmesh]\n",
    "\n",
    "    # load the lobot_merged asset\n",
    "    locobot_template_id = obj_templates_mgr.load_object_configs(\n",
    "        str(os.path.join(data_path, \"objects/locobot_merged\"))\n",
    "    )[0]\n",
    "\n",
    "    # add robot object to the scene with the agent/camera SceneNode attached\n",
    "    id_1 = sim.add_object(locobot_template_id, sim.agents[0].scene_node)\n",
    "    initial_rotation = sim.get_rotation(id_1)\n",
    "\n",
    "    # set the agent's body to kinematic since we will be updating position manually\n",
    "    sim.set_object_motion_type(habitat_sim.physics.MotionType.KINEMATIC, id_1)\n",
    "\n",
    "    # create and configure a new VelocityControl structure\n",
    "    # Note: this is NOT the object's VelocityControl, so it will not be consumed automatically in sim.step_physics\n",
    "    vel_control = habitat_sim.physics.VelocityControl()\n",
    "    vel_control.controlling_lin_vel = True\n",
    "    vel_control.lin_vel_is_local = True\n",
    "    vel_control.controlling_ang_vel = True\n",
    "    vel_control.ang_vel_is_local = True\n",
    "    vel_control.linear_velocity = np.array([0, 0, -1.0])\n",
    "\n",
    "    # try 2 variations of the control experiment\n",
    "    for iteration in range(2):\n",
    "        # reset observations and robot state\n",
    "        observations = []\n",
    "        sim.set_translation(np.array([1.75, -1.02, 0.4]), id_1)\n",
    "        sim.set_rotation(initial_rotation, id_1)\n",
    "        vel_control.angular_velocity = np.array([0.0, 0, 0])\n",
    "\n",
    "        video_prefix = \"robot_control_sliding\"\n",
    "        # turn sliding off for the 2nd pass\n",
    "        if iteration == 1:\n",
    "            sim.config.sim_cfg.allow_sliding = False\n",
    "            video_prefix = \"robot_control_no_sliding\"\n",
    "\n",
    "        # manually control the object's kinematic state via velocity integration\n",
    "        start_time = sim.get_world_time()\n",
    "        last_velocity_set = 0\n",
    "        dt = 6.0\n",
    "        time_step = 1.0 / 60.0\n",
    "        while sim.get_world_time() < start_time + dt:\n",
    "            previous_rigid_state = sim.get_rigid_state(id_1)\n",
    "\n",
    "            # manually integrate the rigid state\n",
    "            target_rigid_state = vel_control.integrate_transform(\n",
    "                time_step, previous_rigid_state\n",
    "            )\n",
    "\n",
    "            # snap rigid state to navmesh and set state to object/agent\n",
    "            end_pos = sim.step_filter(\n",
    "                previous_rigid_state.translation, target_rigid_state.translation\n",
    "            )\n",
    "            sim.set_translation(end_pos, id_1)\n",
    "            sim.set_rotation(target_rigid_state.rotation, id_1)\n",
    "\n",
    "            # Check if a collision occured\n",
    "            dist_moved_before_filter = (\n",
    "                target_rigid_state.translation - previous_rigid_state.translation\n",
    "            ).dot()\n",
    "            dist_moved_after_filter = (end_pos - previous_rigid_state.translation).dot()\n",
    "\n",
    "            # NB: There are some cases where ||filter_end - end_pos|| > 0 when a\n",
    "            # collision _didn't_ happen. One such case is going up stairs.  Instead,\n",
    "            # we check to see if the the amount moved after the application of the filter\n",
    "            # is _less_ than the amount moved before the application of the filter\n",
    "            EPS = 1e-5\n",
    "            collided = (dist_moved_after_filter + EPS) < dist_moved_before_filter\n",
    "\n",
    "            # run any dynamics simulation\n",
    "            sim.step_physics(time_step)\n",
    "\n",
    "            # render observation\n",
    "            observations.append(sim.get_sensor_observations())\n",
    "\n",
    "            # randomize angular velocity\n",
    "            last_velocity_set += time_step\n",
    "            if last_velocity_set >= 1.0:\n",
    "                vel_control.angular_velocity = np.array(\n",
    "                    [0, (random.random() - 0.5) * 2.0, 0]\n",
    "                )\n",
    "                last_velocity_set = 0\n",
    "\n",
    "        # video rendering with embedded 1st person view\n",
    "        if make_video:\n",
    "            make_video_cv2(\n",
    "                observations, prefix=video_prefix, open_vid=show_video, multi_obs=True\n",
    "            )\n",
    "\n",
    "    # [/embodied_agent_navmesh]"
   ]
  }
 ],
 "metadata": {
  "accelerator": "GPU",
  "jupytext": {
   "cell_metadata_filter": "-all",
   "formats": "nb_python//py:percent,colabs//ipynb",
   "main_language": "python",
   "notebook_metadata_filter": "all"
  },
  "kernelspec": {
   "display_name": "Python 3",
   "name": "python3"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}
